#include <avr/io.h>
#include <avr/cpufunc.h>
#include <avr/pgmspace.h>
#include <stdio.h>
#include "Common.h"
#include "tests.h"
#include "Serial.h"
#include "TinyMT.h"

void beginIO()
{
	// Set up for writing to TMS99xx
	DDRA = 0b11111111; // Address low
	DDRC = 0b11111111; // Address high
	DDRH |= 0b00010111; // RD, WR, IORQ, MEMRQ
	PORTH |= 0b00010111; // No mode
}

void endIO()
{
	DDRA = 0b00000000; // Address low
	PORTA = 0b00000000;
	DDRC = 0b00000000; // Address high
	PORTC = 0b00000000;
	DDRD = 0b00000000; // Data
	PORTD = 0b00000000;
	DDRH &= 0b11101000; // RD, WR, IORQ, MREQ
}

uint8_t readIO( uint16_t addr ) {
	// Read status register
	DDRD = 0b00000000;  // Set data input
	PORTD = 0b00000000;
	
	PORTA = addr & 0xff;
	PORTC = addr >> 8;

	PORTH &= 0b11101101; // Activate read and IORQ
	for( uint8_t i = 0; i < 8; i++ ) { _NOP(); }
	uint8_t data = PIND;
	PORTH |= 0b00010010; // Clear read and IORQ

	PORTA = 0b00000000;
	PORTC = 0b00000000;
	return data;
}

void writeIO( uint8_t addr, uint8_t data )
{
	PORTA = addr & 0xff;
	PORTC = addr >> 8;
	DDRD = 0b11111111;
	PORTD = data;
	PORTH &= 0b11101110; // Set write and IORQ
	for( uint8_t i = 0; i < 4; i++ ) { _NOP(); }
	PORTH |= 0b00010001; // Clear write and IORQ
	for( uint8_t i = 0; i < 4; i++ ) { _NOP(); }
	PORTA = 0b00000000;
	PORTC = 0b00000000;
	DDRD = 0b00000000;
	PORTD = 0b00000000;
}

bool writeIOAndWait( uint8_t addr, uint8_t data )
{
	int i;
	PORTA = addr & 0xff;
	PORTC = addr >> 8;
	DDRD = 0b11111111;
	PORTD = data;
	PORTH &= 0b11101110; // Set write and IORQ
	for( i = 0x1000; ( PING & 0b00100000 ) != 0; i-- )
	{
		if( i == 0 )
		{
			printf_P( PSTR( "Wait never asserted\n" ) );
			return false;
		}
	}
	// Wait for WAIT
	for( i = 0x1000; ( PING & 0b00100000 ) == 0; i-- )
	{
		if( i == 0 )
		{
			printf_P( PSTR( "Wait held too long\n" ) );
			return false;
		}
	}
	PORTH |= 0b00010001; // Clear write and IORQ
	for( uint8_t i = 0; i < 4; i++ ) { _NOP(); }
	PORTA = 0b00000000;
	PORTC = 0b00000000;
	DDRD = 0b00000000;
	PORTD = 0b00000000;
	
	return true;
}

uint8_t readVDCStatus()
{
	return readIO( 0xBF );
}

void writeVDCReg( uint8_t reg, uint8_t data )
{
	readVDCStatus(); // Need to read status in order to write to a register

	writeIO( 0xBF, data );
	writeIO( 0xBF, reg | 0b10000000 );
}

void readVRAMAddr( uint16_t addr )
{
	readVDCStatus(); // Need to read status in order to write to a register
	
	writeIO( 0xBF, addr & 0xff );
	writeIO( 0xBF, ( addr >> 8 ) | 0b00000000 );
}

void writeVRAMAddr( uint16_t addr )
{
	readVDCStatus(); // Need to read status in order to write to a register
	
	writeIO( 0xBF, addr & 0xff );
	writeIO( 0xBF, ( addr >> 8 ) | 0b01000000 );
}

uint8_t readVRAM( )
{
	return readIO( 0xBE );
}

void writeVRAM( uint8_t data )
{
	writeIO( 0xBE, data );
}

bool writeSound( uint8_t channel, int divider, uint8_t volume)
{
	if( !writeIOAndWait( 0xff, 0b10000000 | (channel << 5) | (divider & 0b00001111) ) )
	{
		return false;
	}
	if( channel != 3 )
	{
		if( !writeIOAndWait( 0xff, 0b00000000 | (divider >> 4) ) )
		{
			return false;
		}
	}
	if( !writeIOAndWait( 0xff, 0b10010000 | (channel << 5) | volume ) )
	{
		return false;
	}

	return true;
}

bool vramTest() {
	if( !doBusAck() )
	{
		return false;
	}

	printf_P( PSTR( "Testing video memory\n" ) );
	
	beginIO();

	// Set mode 0 with 16K memory, no display
	writeVDCReg( 0, 0b00000000 );
	writeVDCReg( 1, 0b10000000 );


	uint8_t errors1 = 0;
	for( uint8_t i = 0; i < 21; i++ )
	{
		tinymt32_t state;
		
		printf_P( PSTR( "Memory test pattern %u: " ), i );

		tinymt32_init( &state, i );
		
		writeVRAMAddr( 0x0000 );
		for( uint16_t i = 0; i < 0x4000; i++ )
		{
			writeVRAM( (uint8_t) tinymt32_generate_uint32( &state ) );
		}

		delay( 500 );  // Give RAM time to degrade

		// Now read back
		tinymt32_init( &state, i );

		readVRAMAddr( 0x0000 );
		for( uint16_t i = 0; i < 0x4000; i++ )
		{
			uint8_t data = readVRAM( ) ^ (uint8_t) tinymt32_generate_uint32( &state );
			errors1 |= data;
		}

		if( errors1 != 0 )
		{
			printf_P( PSTR( "Errors encountered\n" ) );
			break;
		}
		
		printf_P( PSTR( "OK\n" ) );
	}

	bool pass = ( errors1 == 0 );
	
	for( uint8_t i = 0; errors1 != 0; i++, errors1 >>= 1 )
	{
		if( ( errors1 & 0b00000001 ) != 0 ) {
			static char const vram[][5] PROGMEM = {
				"U16 ", "U14 ", "U12 ", "U10 ", "U17 ", "U15 ", "U13 ", "U11 "
			};
			printf_P( vram[i] );
		}
	}

	if( pass ) {
		printf_P( PSTR( "VRAM test pass\n" ) );
	}
	else
	{
		printf_P( PSTR( " VRAM chips read incorrect\n" ) );
	}
	
	endIO();

	return pass;
}

static void printJoy( uint8_t in_num, uint8_t in_val ) {
	printf_P( PSTR( "%u: " ), in_num );
	for( uint8_t i = 0; i < 8; i++, in_val >>= 1 ) {
		static char const dirs[][3] PROGMEM = {
			"N", "E", "S", "W", "Qt", "Ql", "B", "Qr"
		};
		printf_P( PSTR( "%S%S" ), dirs[i],
		( ( in_val & 0b00000001 ) == 0 ) ? PSTR( "+ " ) : PSTR( "- " ) );
	}
	printf_P( PSTR( "\n" ) );
}

static void printKey( uint8_t in_num, uint8_t in_val ) {
	printf_P( PSTR( "%u: " ), in_num );
	static char const keys[][6] PROGMEM = {
		"E0   ", "Key 8", "Key 4", "Key 5",
		"BLU  ", "Key 7", "Key #", "Key 2",
		"VIO  ", "Key *", "Key 0", "Key 9",
		"Key 3", "Key 1", "Key 6", "     "
	};
	printf_P( PSTR( "%S       " ), keys[in_val & 0xf] );
	in_val >>= 4;
	for( int i = 0; i < 4; i++, in_val >>= 1 ) {
		static char const quad[][3] PROGMEM = {
			 "Qt", "Ql", "B", "Qr" 
		};
		printf_P( PSTR( "%S%S" ), quad[i],
		( ( in_val & 0b00000001 ) == 0 ) ? PSTR( "+ " ) : PSTR( "- " ) );
	}
	printf_P( PSTR( "\n" ) );
}

uint8_t joy0[16];
uint8_t joy1[16];
uint8_t key0[16];
uint8_t key1[16];

bool checkInput() {
	// We're going to keep printing until another input is received.  So clear the buffer of any
	// immediate data
	delay(100);
	while( serial_available_f(stdin) > 0 ) {
		getchar();
		delay(100);
	}

	bool pass = doBusAck();
	if( !pass ) {
		return pass;
	}

	// Set up for talking to controller ports
	DDRA = 0b11111111; // Address low
	DDRC = 0b11111111; // Address high
	DDRD = 0b00000000; // Data
	DDRH |= 0b00010111; // RD, WR, IORQ, MREQ
	PORTH |= 0b00010111; // No mode
	
	// Test that the quad input triggers INT
	// Start by testing that INT is not triggered
	if( ( PINE & 0b00000100 ) == 0 ) {
		printf_P( PSTR( "INT line is already active\n" ) );
		pass = false;
	}
	DDRB |= 0b00100000;
	PORTB |= 0b00100000;
	for( int i = 0; i < 16; i++ ) { _NOP(); }
	if( ( PINE & 0b00000100 ) != 0 ) {
		pass = false;
		printf_P( PSTR( "QUAD->INT trigger fail\n" ) );
	}
	DDRB &= 0b11011111;
	PORTB &= 0b11011111;
	// Wait for INT line to fall again
	while( ( PINE & 0b00000100 ) == 0 );

	uint8_t sweep = 0;
	bool last_int = false;
	uint8_t last_joy0 = 0;
	uint8_t last_joy1 = 0;
	uint8_t last_key0 = 0;
	uint8_t last_key1 = 0;
	while( serial_available_f(stdin) == 0 ) {
		writeIO( 0xC0, 0 ); // Read joysticks
		for( int i = 0; i < 64; i++ ) { _NOP(); }
		joy0[sweep] = readIO( 0xFC );
		joy1[sweep] = readIO( 0xFF );

		writeIO( 0x80, 0 ); // Read keypads
		for( int i = 0; i < 8; i++ ) { _NOP(); }
		key0[sweep] = readIO( 0xFC );
		key1[sweep] = readIO( 0xFF );

		sweep++;
		sweep &= 0xf;

		// See if the sweep is debounced
		if( sweep == 0) {
			for( sweep = 0; sweep < 15; sweep++ ) {
				if( (joy0[sweep] & 0b00101111) != (joy0[sweep+1] & 0b00101111) ) {
					break;
				}
			}
			if( sweep == 15 ) {
				// Joystick 0 is stable
				if( (last_joy0 & 0b01111111) != (joy0[0] & 0b01111111) ) {
					last_joy0 = joy0[0];
					printJoy( 1, last_joy0 );
				}
			}

			for( sweep = 0; sweep < 15; sweep++ ) {
				if( (joy1[sweep] & 0b00101111) != (joy1[sweep+1] & 0b00101111) ) {
					break;
				}
			}
			if( sweep == 15 ) {
				// Joystick 1 is stable
				if( (last_joy1 & 0b01111111) != (joy1[0] & 0b01111111) ) {
					last_joy1 = joy1[0];
					printJoy( 2, last_joy1 );
				}
			}

			for( sweep = 0; sweep < 15; sweep++ ) {
				if( (key0[sweep] & 0b00101111) != (key0[sweep+1] & 0b00101111) ) {
					break;
				}
			}
			if( sweep == 15 ) {
				// Keypad 0 is stable
				if( (last_key0 & 0b01111111) != (key0[0] & 0b01111111) ) {
					last_key0 = key0[0];
					printKey( 1, last_key0 );
				}
			}

			for( sweep = 0; sweep < 15; sweep++ ) {
				if( (key1[sweep] & 0b00101111) != (key1[sweep+1] & 0b00101111) ) {
					break;
				}
			}
			if( sweep == 15 ) {
				// Keypad 1 is stable
				if( (last_key1 & 0b01111111) != (key1[0] & 0b01111111) ) {
					last_key1 = key1[0];
					printKey( 2, last_key1 );
				}
			}

			sweep = 0;
		}

		bool new_int = (PINE & 0b00000100) == 0;
		if( last_int != new_int ) {
			last_int = new_int;
			printf_P( PSTR( "Quad interrupt %S\n" ), last_int ? PSTR( "active" ) : PSTR( "inactive" ) );
		}
	}

	getchar();

	return pass;
}

bool soundTest()
{
	// We're going to get user input for each stage
	delay(100);
	while( serial_available_f(stdin) > 0 ) {
		getchar();
		delay(100);
	}

	bool pass = doBusAck();
	if( !pass ) {
		return pass;
	}

	beginIO();

	for( int i = 0x1000; ( PING & 0b00100000 ) == 0; i-- )
	{
		if( i == 0 )
		{
			printf_P( PSTR( "Wait is being held\n" ) );
			endIO();
			return false;
		}
	}
	
	if( !writeSound( 0, 1, 15 ) ||
	!writeSound( 1, 1, 15 ) ||
	!writeSound( 2, 1, 15 ) ||
	!writeSound( 3, 0, 15 ) )
	{
		printf_P( PSTR( "Sound chip did not respond properly\n" ) );
		endIO();
		return false;
	}

	static int const scale[] PROGMEM = {
		427, 380, 339, 320, 285, 254, // C, D, E, F, G, A
		226, 213, 190, 169, 151, 142, // B, C, D, E, F, G
	};
	static char const l0[] PROGMEM = "C major";
	static char const l1[] PROGMEM = "D minor";
	static char const l2[] PROGMEM = "E minor";
	static char const l3[] PROGMEM = "F major";
	static char const l4[] PROGMEM = "G major";
	static char const l5[] PROGMEM = "A minor";
	static char const l6[] PROGMEM = "B diminished";
	static char const * const chords[] PROGMEM = {
		l0, l1, l2, l3, l4, l5, l6, l0
	};
	
	for( uint8_t i = 0; i < 8; i++ )
	{
		printf_P( PSTR( "Playing %S chord and %S noise %d\n" ), pgm_read_ptr( &chords[i] ), (i < 4) ? PSTR( "periodic" ) : PSTR( "white" ), i & 3 );
		for( uint8_t j = 0; j < 3; j++ )
		{
			if( !writeSound( j, pgm_read_word( &scale[i + j * 2] ), 0 ) )
			{
				printf_P( PSTR( "Sound chip did not respond properly\n" ) );
				pass = false;
			}
		}
		if( !writeSound( 3, i, 0 ) )
		{
			printf_P( PSTR( "Sound chip did not respond properly\n" ) );
			pass = false;
		}
		
		getchar();
	}
	
	writeSound( 0, 1, 15 );
	writeSound( 1, 1, 15 );
	writeSound( 2, 1, 15 );
	writeSound( 3, 0, 15 );

	for( uint8_t c = 0; c < 3; c++ )
	{
		printf_P( PSTR( "Testing channel %u attenuation\n" ), c );
		
		while( !serial_available_f(stdin) )
		{
			for( uint8_t v = 0; v < 16; v++ )
			{
				writeSound( c, pgm_read_word( &scale[c * 2] ), v );
				delay( 200 );
			}
		}
		
		getchar();
	}
	printf_P( PSTR( "Testing noise attenuation\n" ) );

	while( !serial_available_f(stdin) )
	{
		for( uint8_t v = 0; v < 16; v++ )
		{
			writeSound( 3, 5, v );
			delay( 200 );
		}
	}
	
	getchar();

	endIO();

	return pass;
}